#pragma once

#include <ros/ros.h>
#include "stair_ctrl/JoyStick.h"

#include <termios.h>
#include <iostream>


int getch()
{
	int ch;
	struct termios oldt;
	struct termios newt;

	// Store old settings, and copy to new settings
	tcgetattr(STDIN_FILENO, &oldt);
	newt = oldt;

	// Make required changes and apply the settings
	newt.c_lflag &= ~(ICANON | ECHO);
	newt.c_iflag |= IGNBRK;
	newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
	newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
	newt.c_cc[VMIN] = 0;
	newt.c_cc[VTIME] = 1;
	tcsetattr(fileno(stdin), TCSANOW, &newt);

	// Get the current character
	ch = getchar();

	// Reapply old settings
	tcsetattr(STDIN_FILENO, TCSANOW, &oldt);

	return ch;
}

class keyboardCtrl
{
private:
    ros::Publisher fake_js_pub;
    stair_ctrl::JoyStick joy_stick;

public:
    keyboardCtrl(ros::NodeHandle *nh, std::string js_topic)
    {
        fake_js_pub = nh->advertise<stair_ctrl::JoyStick>(js_topic, 10);
    }

    void updateJs(int k)
    {
        joy_stick = stair_ctrl::JoyStick();
        switch (k)
        {
        case 'w':
            joy_stick.ly = 0.5;
            break;
        case 's':
            joy_stick.ly = -0.5;
            break;
        case 'a':
            joy_stick.lx = -0.5;
            break;
        case 'd':
            joy_stick.lx = 0.5;
            break;
        case 'i':
            joy_stick.ry = 0.5;
            break;
        case 'k':
            joy_stick.ry = -0.5;
            break;
        case 'j':
            joy_stick.rx = -0.5;
            break;
        case 'l':
            joy_stick.rx = 0.5;
            break;
        case '1':
            joy_stick.keys.L2 = 1;
            break;
        case '2':
            joy_stick.keys.L1 = 1;
            break;
        case '9':
            joy_stick.keys.R1 = 1;
            break;
        case '0':
            joy_stick.keys.R2 = 1;
            break;
        case '=':
            joy_stick.keys.start = 1;
            break;
        
        default:
            break;
        }
    }

    void start(int rate)
    {
        ros::Rate loop_rate(rate);
        
        while (ros::ok())
        {
            loop_rate.sleep();
            int k = getch();
            if (k == '.')
            {
                break;
            }
            updateJs(k);
            fake_js_pub.publish(joy_stick);
        }
    }
};